#include "Joint.h"

namespace poly
{
	Joint::Joint() :
		rigidBody1( 0 ), rigidBody2( 0 ),
			pointOfApp1( 0, 0, 0 ), pointOfApp2( 0, 0, 0 )
	{
	}

	void Joint::apply(float dt)
	{
		float3 pt1 = transformCoord( pointOfApp1, rigidBody1->world() );
		float3 pt2 = transformCoord( pointOfApp2, rigidBody2->world() );

		float3 d = pt2 - pt1;

		float ttlInvMass = rigidBody1->invMass + rigidBody2->invMass;

		float3 d1 = +rigidBody1->invMass * d / ttlInvMass;
		float3 d2 = -rigidBody2->invMass * d / ttlInvMass;
		rigidBody1->displace( d1 );
		rigidBody2->displace( d2 );

		rigidBody1->resolveJoint( *rigidBody2, pt1 + d1 );
	}
}
